﻿using System;
using System.Drawing;
using System.Drawing.Imaging;
using System.Collections.Generic;
using AForge.Imaging.Filters;
using InvariantWM.NormalizerUtils;

namespace InvariantWM
{
    class CornerDetector
    {
        int diameter = InvariantWM.Properties.Settings.Default.diameter;
        // brightness difference threshold
        private int differenceThreshold = 25;
        // geometrical threshold
        private int geometricalThreshold = 18;

        /// <summary>
        /// Brightness difference threshold.
        /// </summary>
        ///
        /// <remarks><para>The brightness difference threshold controls the amount
        /// of pixels, which become part of USAN area. If difference between central
        /// pixel (nucleus) and surrounding pixel is not higher than difference threshold,
        /// then that pixel becomes part of USAN.</para>
        /// <para>Increasing this value increases the amount of detected corners.</para>
        /// <para>Default value is set to 25.</para>
        /// </remarks>
        ///
        public int DifferenceThreshold
        {
            get { return differenceThreshold; }
            set { differenceThreshold = value; }
        }

        /// <summary>
        /// Geometrical threshold.
        /// </summary>
        ///
        /// <remarks><para>The geometrical threshold sets the maximum number of pixels
        /// in USAN area around corner. If potential corner has USAN with more pixels, than
        /// it is not a corner.</para>
        /// <para> Decreasing this value decreases the amount of detected corners - only sharp corners
        /// are detected. Increasing this value increases the amount of detected corners, but
        /// also increases amount of flat corners, which may be not corners at all.</para>
        /// <para>Default value is set to 18, which half of maximum amount of pixels in USAN.</para>
        /// </remarks>
        ///
        public int GeometricalThreshold
        {
            get { return geometricalThreshold; }
            set { geometricalThreshold = value; }
        }
       
        private static int[] rowRadius = new int[7] { 1, 2, 3, 3, 3, 2, 1 };

        /// <summary>
        /// Initializes a new instance of the <see cref="SusanCornersDetector"/> class.
        /// </summary>
        public CornerDetector( )
        {
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="SusanCornersDetector"/> class.
        /// </summary>
        ///
        /// <param name="differenceThreshold">Brightness difference threshold.</param>
        /// <param name="geometricalThreshold">Geometrical threshold.</param>
        ///
        public CornerDetector( int differenceThreshold, int geometricalThreshold )
        {
            this.differenceThreshold = differenceThreshold;
            this.geometricalThreshold = geometricalThreshold;
        }

        /// <summary>
        /// Process image looking for corners.
        /// </summary>
        ///
        /// <param name="image">Source image to process.</param>
        ///
        /// <returns>Returns array of found corners (X-Y coordinates).</returns>
        ///
        /// <exception cref="ArgumentException">The source image has incorrect pixel format.</exception>
        ///
        public Point[] ProcessImage( Bitmap image )
        {
            // check image format
            if (
                ( image.PixelFormat != PixelFormat.Format8bppIndexed ) &&
                ( image.PixelFormat != PixelFormat.Format24bppRgb )
                )
            {
                throw new ArgumentException( "Source image can be grayscale (8 bpp indexed) or color (24 bpp) image only" );
            }

            // lock source image
            BitmapData imageData = image.LockBits(
                new Rectangle( 0, 0, image.Width, image.Height ),
                ImageLockMode.ReadOnly, image.PixelFormat );

            // process the image
            Point[] corners = ProcessImage( imageData );

            // unlock image
            image.UnlockBits( imageData );

            return corners;
        }

        /// <summary>
        /// Process image looking for corners.
        /// </summary>
        ///
        /// <param name="imageData">Source image data to process.</param>
        ///
        /// <returns>Returns array of found corners (X-Y coordinates).</returns>
        ///
        /// <exception cref="ArgumentException">The source image has incorrect pixel format.</exception>
        ///
        public Point[] ProcessImage( BitmapData imageData )
        {
            // check image format
            if (
                ( imageData.PixelFormat != PixelFormat.Format8bppIndexed ) &&
                ( imageData.PixelFormat != PixelFormat.Format24bppRgb )
                )
            {
                throw new ArgumentException( "Source image can be grayscale (8 bpp indexed) or color (24 bpp) image only" );
            }

            // get source image size
            int width  = imageData.Width;
            int height = imageData.Height;

            // make sure we have grayscale image
            Bitmap grayImage = null;
            BitmapData grayImageData = null;

            if ( imageData.PixelFormat == PixelFormat.Format8bppIndexed )
            {
                grayImageData = imageData;
            }
            else
            {
                // create temporary grayscale image
                GrayscaleBT709 grayFilter = new GrayscaleBT709( );
                grayImage = grayFilter.Apply( imageData );
                grayImageData = grayImage.LockBits( new Rectangle( 0, 0, width, height ),
                    ImageLockMode.ReadOnly, PixelFormat.Format8bppIndexed );
            }


            int[,] susanMap = new int[height, width];

            // do the job
            unsafe
            {
                int stride = grayImageData.Stride;
                int offset = stride - width;

                byte* src = (byte*) grayImageData.Scan0.ToPointer( ) + stride * 3 + 3;

                            // for each row
                for ( int y = 3, maxY = height - 3; y < maxY; y++ )
                {
                    // for each pixel
                    for ( int x = 3, maxX = width - 3; x < maxX; x++, src++ )
                    {
                        // get value of the nucleus
                        byte nucleusValue = *src;
                        // usan - number of pixels with similar brightness
                        int usan = 0;
                        // center of gravity
                        int cx = 0, cy = 0;

                        // for each row of the mask
                        for ( int i = -3; i <= 3; i++ )
                        {
                            // determine row's radius
                            int r = rowRadius[i + 3];

                            // get pointer to the central pixel of the row
                            byte* ptr = src + stride * i;

                            // for each element of the mask's row
                            for ( int j = -r; j <= r; j++ )
                            {
                                // differenceThreshold
                                if ( System.Math.Abs( nucleusValue - ptr[j] ) <= differenceThreshold )
                                {
                                    usan++;

                                    cx += x + j;
                                    cy += y + i;
                                }
                            }
                        }

                        // check usan size
                        if ( usan < geometricalThreshold )
                        {
                            cx /= usan;
                            cy /= usan;

                            if ( ( x != cx ) || ( y != cy ) )
                            {
                                // cornersList.Add( new Point( x, y ) );
                                usan = ( geometricalThreshold - usan );
                            }
                            else
                            {
                                usan = 0;
                            }
                        }
                        else
                        {
                            usan = 0;
                        }

                        // usan = ( usan < geometricalThreshold ) ? ( geometricalThreshold - usan ) : 0;
                        susanMap[y, x] = usan;
                    }

                    src += 6 + offset;
                }
            }

            // free temporary image if required
            if ( grayImage != null )
            {
                grayImage.UnlockBits( grayImageData );
                grayImage.Dispose( );
            }

            // collect interesting points - only those points, which are local maximums
            List<Point> cornersList = new List<Point>( );
            List<Corner> cornersList2 = new List<Corner>();

            // for each row
            for ( int y = 2, maxY = height - 2; y < maxY; y++ )
            {
                // for each pixel
                for ( int x = 2, maxX = width - 2; x < maxX; x++ )
                {
                    int currentValue = susanMap[y, x];

                    // for each windows' row
                    for ( int i = -2; ( currentValue != 0 ) && ( i <= 2 ); i++ )
                    {
                        // for each windows' pixel
                        for ( int j = -2; j <= 2; j++ )
                        {
                            if ( susanMap[y + i, x + j] > currentValue )
                            {
                                currentValue = 0;
                                break;
                            }
                        }
                    }

                    // check if this point is really interesting
                    if ( currentValue != 0 )
                    {
                        //cornersList.Add( new Point( x, y ) );
                        Corner corner = new Corner();
                        corner.Priority = currentValue;
                        corner.Punto = new Point (x, y);
                        cornersList2.Add(corner);
                    }
                }
            }

            // Ordenar
            cornersList2.Sort(CompareCorner);

            // convert list to array
            //Point[] corners = new Point[cornersList.Count];
            List<Point> corners2 = new List<Point>();
            //Point[] corners2 = new Point[InvariantWM.Properties.Settings.Default.cornersNr];
            //cornersList.CopyTo( corners );
            int count = 0;
            bool isin;
            foreach (Corner corner in cornersList2)
            {
                isin=true;

                foreach (Point corner2 in corners2)
                {

                    if (Point2D.point2dDistance(corner.Punto.X, corner.Punto.Y, corner2.X, corner2.Y) < diameter * 2)
                    {
                        isin=false;
                    }

                    if (corner.Punto.X < diameter || corner.Punto.X > height - diameter || corner.Punto.Y < diameter || corner.Punto.Y > width - diameter)
                    {
                        isin = false;
                    }
                }
                if (isin) {
                    corners2.Add(corner.Punto);
                    count++;
                }
                if (count > InvariantWM.Properties.Settings.Default.cornersNr - 1) break;
            }

            //return corner
            Point[] cornersFinal = new Point[corners2.Count];
            int ipoint = 0;
            foreach(Point point in corners2)
            {
                cornersFinal[ipoint] = point;
                ipoint++;
            }
            return cornersFinal;
        }

        public static int CompareCorner(Corner c1, Corner c2)
        {
            double priority1 = c1.Priority;
            double priority2 = c2.Priority;
            int comp = priority1.CompareTo(priority2);

            if (priority1 > priority2)
            {
                return -1;
            }
            else if (priority2 > priority1)
            {
                return 1;
            }
            else
            {
                return 0;
            }
        }
    }
}
